#ifndef     __ARM_MOTOR_H
#define     __ARM_MOTOR_H

#include "../../utils/error.h"
#include "../../conn/can_frame.h"
#include "../../utils/nocopyable.h"
#include "../../utils/object.h"

namespace driver {

struct ArmInfo {
    uint8_t id;
    uint8_t err;
    int16_t pos; // 角度
    uint16_t speed; // 角度/s
    float torque; // 扭矩
    uint8_t mos_temp; // MOS管温度
    uint8_t coil_temp; // 电机线圈温度
};

class ArmMotor : public utils::NoCopyable {

public:
    static constexpr uint16_t M1_ID = 0x100 + 1;
    static constexpr uint16_t M2_ID = 0x100 + 2;

    static constexpr uint32_t RUN_TIMEOUT = 10 * 1000;

    static constexpr uint16_t STD_SPEED = 900;

    ArmMotor();

    void status_update();
    utils::Error disable();
    utils::Error enable();
    utils::Error adj();

    utils::Error init() {
        return run(990, 912);
    }

    utils::Error run(uint16_t p1, uint16_t p2, uint16_t speed = STD_SPEED);

    utils::Error run_motor1(uint16_t p1, uint16_t speed);
    utils::Error run_motor2(uint16_t p2, uint16_t speed);

    const ArmInfo &info1() { return m_m1_info; }
    const ArmInfo &info2() { return m_m2_info; }

private:
    static void parse_info(const conn::CANFrame &frame, ArmInfo &info);
    
    utils::Error write(uint16_t id, const uint8_t *buf8);
    ArmInfo m_m1_info;
    ArmInfo m_m2_info;
};

inline utils::Object<ArmMotor> armMotor;

}

#endif
